package monitor;

import java.util.ArrayList;
import java.util.List;

import robotRouting.FindAPebble;
import robotRouting.NodeRouting;

public class dataDrivenController implements WholeBlockEventListener {
	// lost search function is added..
	// cannot see any pebble lost error is fixed. !

	public static boolean isSpinning = false;
	public static int spinCounter = 0;

	private Decoder decoder;
	private NodeList globalNodeList = new NodeList();
	private ArrayList<NodeWithAngle> localNodeList = new ArrayList<NodeWithAngle>();
	private int[] channelInfoBuffer = new int[10];

	private boolean isRouteSet = false;
	private boolean isRouteSetAvailable = false;

	private boolean isLostContinues = false;
	private int isLostCount = 7;
	private boolean isCurrentLostContinues = false;
	private int isCurrentLostCount = 8;
	private boolean isLost = false;
	private int searchCount = 0;
	private int searchTimerCount = 0;

	private int moveCount=1;
	private int proceedCount = 0;

	// added by Mi for voice control
	private boolean isSpecifyPlayed = false;
	private boolean isProceedingPlayed = true;
	private boolean isSearchingPlayed = true;

	private void resetLostInfo() {
		isLostContinues = false;
		isLostCount = 7;
		isCurrentLostContinues = false;
		isCurrentLostCount = 8;
		isLost = false;
		searchCount = 0;
		searchTimerCount = 0;
	}

	private int currentTargetPebbleId = -1; // used as a temp target.
	private int finalTargetPebbleId;
	private ArrayList<Integer> routeArray = new ArrayList<Integer>();
	private ArrayList<BackupRouteNode> backRouteArray = new ArrayList<BackupRouteNode>();
	private ArrayList<Integer> searchTargetArray = new ArrayList<Integer>();
	private ArrayList<Integer> targetPebbleDegreeArray = new ArrayList<Integer>();

	private ArrayList<Integer> locateArray = new ArrayList<Integer>();
	private boolean isLocate = false;

	private int movingCount = 4;
	private boolean checkPointPlayed = false;

	// private int reachCount=0;

	public void locatePebbleNetwork() {
		locateArray.clear();
		isRouteSetAvailable = true;
		isLocate = true;

		while (isLocate) {
			System.out.println("wait for locate");
		}

		FindAPebble.sndplayer.playLocate(locateArray);
		resetRobot();

	}

	private List<AnglesEventListener> anglesListeners = new ArrayList<AnglesEventListener>();

	public void addAnglesEventListener(AnglesEventListener listener) {
		anglesListeners.add(listener);
	}

	public void removeAnglesEventListener(AnglesEventListener listener) {
		anglesListeners.remove(listener);
	}

	protected void notifyAnglesEventListeners(AnglesEvent e) {
		for (AnglesEventListener listener : anglesListeners) {
			listener.anglesReady(e);
		}
	}

	public void setFinalTargetPebbleId(int id) {
		this.finalTargetPebbleId = id;
		// info should be reset here.
		isRouteSetAvailable = true;
		isRouteSet = false;
		isLostContinues = false;
		isCurrentLostContinues = false;
		routeArray.clear();
		backRouteArray.clear();
		targetPebbleDegreeArray.clear();
		currentTargetPebbleId = -1;
		movingCount = 4;
		checkPointPlayed = false;
		resetLostInfo();
		moveCount=1;
		// reachCount=0;
	}

	public void resetRobot() {

		isRouteSetAvailable = false;
		isRouteSet = false;
		isLostContinues = false;
		isCurrentLostContinues = false;
		routeArray.clear();
		backRouteArray.clear();
		targetPebbleDegreeArray.clear();
		FindAPebble.isRunning = false;
		currentTargetPebbleId = -1;
		movingCount = 4;
		checkPointPlayed = false;
		resetLostInfo();
		// reachCount=0;
		isSpinning = false;
		spinCounter = 0;
		moveCount=1;
	}

	public dataDrivenController(Decoder decoder, NodeList globalNodeList,
			ArrayList<NodeWithAngle> localNodeList, int[] channelInfoBuffer) {
		super();
		this.decoder = decoder;
		this.globalNodeList = globalNodeList;
		this.localNodeList = localNodeList;
		this.channelInfoBuffer = channelInfoBuffer;
	}

	public dataDrivenController() {
		super();
	}

	public void initDecoder() {
		decoder = new Decoder();
		decoder.initSerial();
		decoder.addBlockListener(this);
	}

	@Override
	public void completeDataArriaved(WholeBlockEvent e) {

		// added by Mi for voice control
		if (!isSpecifyPlayed) {
			FindAPebble.sndplayer.play("specify");
			isSpecifyPlayed = true;
		} else if (!isProceedingPlayed) {
			if (proceedCount < 2) {
				proceedCount++;
			} else {
				FindAPebble.sndplayer.play("proceeding");
				isProceedingPlayed = true;
				proceedCount = 0;
			}
		} else if (!isSearchingPlayed) {
			FindAPebble.sndplayer.play("Searching");
			isSearchingPlayed = true;
		}




		System.out
				.println("--------------DataBlock starting Mark-----------------");
		System.out.println("Before Symmetry Ajustion------------------------");

		globalNodeList = e.getGlobalNodeList();
		localNodeList = e.getLocalNodeList();
		channelInfoBuffer = e.getChannelInfoBuffer();
		int globalInfoSize = globalNodeList.size();
		System.out.println("Global topology datablock size:" + globalInfoSize);

		for (int i = 0; i < globalInfoSize; i++) {
			Node tempNode = globalNodeList.getNode(i);
			int id = tempNode.getId();
			int ver = tempNode.getVer();
			int pressed = tempNode.getPressed();
			List<Neibour> neibours = tempNode.getNeibours();
			int neibourSize = neibours.size();

			System.out.print(id + " " + ver + " " + pressed + " " + neibourSize
					+ "    ");

			for (int j = 0; j < neibourSize; j++) {
				int neiboursId = neibours.get(j).getId();
				System.out.print(neiboursId + " ");

				for (int k = 0; k < globalInfoSize; k++) {
					boolean isSymm = false;
					Node tempNode1 = globalNodeList.getNode(k);
					int id1 = tempNode1.getId();

					if (id1 == neiboursId) {
						List<Neibour> neibours1 = tempNode1.getNeibours();
						int neibourSize1 = neibours1.size();

						for (int p = 0; p < neibourSize1; p++) {
							if (neibours1.get(p).getId() == id) {
								isSymm = true;
								break;
							}
						}
						if (!isSymm) {
							Neibour n = new Neibour(id);
							neibours1.add(n);
							isSymm = true;
						}

					}

				}

			}
			System.out.println();
		}

		System.out
				.println("After Symmetry Ajustion----------------------------");

		for (int i = 0; i < globalInfoSize; i++) {
			Node tempNode = globalNodeList.getNode(i);
			int id = tempNode.getId();
			int ver = tempNode.getVer();
			int pressed = tempNode.getPressed();
			List<Neibour> neibours = tempNode.getNeibours();
			int neibourSize = neibours.size();

			System.out.print(id + " " + ver + " " + pressed + " " + neibourSize
					+ "    ");

			if (!FindAPebble.isRunning && !isRouteSet && pressed == 1) {
				FindAPebble.dataController.setFinalTargetPebbleId(id);
				FindAPebble.isRunning = true;
			}

			if (isLocate)
				locateArray.add(id);

			for (int j = 0; j < neibourSize; j++) {
				System.out.print(neibours.get(j).getId() + " ");

			}
			System.out.println();

		}

		if (!isRouteSetAvailable) {
			System.out.println("Waiting for the target setting!!!!");
			return;
		}

		System.out.println("target already set!!!!!!!!!!!!");

		if (isLocate) {
			isLocate = false;
			isRouteSetAvailable = false;
			return;
		}

		int localInfoSize = localNodeList.size();
		System.out.println("Local datainfo size Before Filtering :"
				+ localInfoSize);
		for (int i = 0; i < localInfoSize; i++) {
			System.out.println(localNodeList.get(i).getId() + " "
					+ localNodeList.get(i).getAngle() + " "
					+ localNodeList.get(i).getChannel());

		}

		System.out.println("Local datainfo after Filtering:");
		int pos = 0;
		for (int i = 0; i < localInfoSize; i++) {
			int id = localNodeList.get(pos).getId();

			if (channelInfoBuffer[id] == 0) {
				localNodeList.remove(pos);
				System.out.println(id + " is removed ");
			} else {
				System.out.println(id + " " + localNodeList.get(pos).getAngle()
						+ " " + localNodeList.get(pos).getChannel());
				pos++;
			}
		}

		System.out.println("Local datainfo size after Filtering "
				+ localNodeList.size());

		System.out.println("BufferInfo:");

		for (int i = 0; i < 10; i++)
			System.out.println(channelInfoBuffer[i]);

		System.out.println("isLostContinues: " + isLostContinues);
		System.out.println("isLostCount: " + isLostCount);
		System.out.println("isCurrentLostContinues: " + isCurrentLostContinues);
		System.out.println("isCurrentLostCount: " + isCurrentLostCount);
		System.out.println("isLost: " + isLost);
		System.out.println("searchCount: " + searchCount);
		System.out.println("searchTimerCount: " + searchTimerCount);

		if (isSpinning) {
			System.out.println("robot is spinning");
			if (spinCounter++ > 1) {
				this.isSpinning = false;
				spinCounter = 0;
			}
			System.out
					.println("---------------DataBlock end Mark----------------");
			return;
		}

		System.out.println("---------------DataBlock end Mark----------------");

		if (!isRouteSet) {



			int pebbleNumInSight = localNodeList.size();
			if (pebbleNumInSight == 0) {
				System.out.println("The robot cannot see any pebble!!");
				FindAPebble.sndplayer.playNopebble();
				isRouteSetAvailable = false;
				FindAPebble.isRunning = false;
				resetRobot(); // new added
			} else {

				for (int i = 0; i < pebbleNumInSight; i++) {

					int id = localNodeList.get(i).getId();
					NodeRouting nr = new NodeRouting(id, finalTargetPebbleId,
							globalNodeList);
					if (nr.findPath()) {
						System.out.println("yes!!!!!! A path is found!!! from "
								+ id + " to " + finalTargetPebbleId);

						FindAPebble.roombacomm.control();

						// play find the Path
						// FindAPebble.roombacomm.playNote(90, 10); // E
						// FindAPebble.roombacomm.pause(200);
						// FindAPebble.roombacomm.playNote(100, 10); // C
						// FindAPebble.roombacomm.pause(200);
						// FindAPebble.roombacomm.playNote(105, 10); // G
						// FindAPebble.roombacomm.pause(200);
						FindAPebble.roombacomm.playNote(72, 10); // C
						FindAPebble.roombacomm.pause(200);
						FindAPebble.roombacomm.playNote(76, 10); // E
						FindAPebble.roombacomm.pause(200);
						FindAPebble.roombacomm.playNote(79, 10); // G
						FindAPebble.roombacomm.pause(200);

						FindAPebble.sndplayer.playGoal(finalTargetPebbleId);

						// sound should be play to indicate a path is found!
						// FindAPebble.roombacomm.playNote(72, 10); // C
						// FindAPebble.roombacomm.pause(200);
						// FindAPebble.roombacomm.playNote(79, 10); // G
						// FindAPebble.roombacomm.pause(200);
						// FindAPebble.roombacomm.playNote(76, 10); // E
						// FindAPebble.roombacomm.pause(200);

						nr.reslutDisplay();
						routeArray = nr.getResultList();

						if (routeArray.size() > 0) {
							for (int m = 0; m < routeArray.size(); m++) {
								backRouteArray.add(new BackupRouteNode(
										routeArray.get(m), false));
							}
							System.out
									.println("BackupRouteArrayList initial info:");
							for (int m = 0; m < backRouteArray.size(); m++) {
								System.out.println(backRouteArray.get(m)
										.toString());
							}

						}

						// do not play route
						FindAPebble.sndplayer.playRoute(routeArray);

						currentTargetPebbleId = id;
						resetLostInfo();
						routeArray.remove(routeArray.size() - 1);
						isRouteSet = true;
						return;
					} else {
						System.out.println("No path can be found from " + id
								+ " to " + finalTargetPebbleId);
					}

				}

				if (!isRouteSet) {
					isRouteSetAvailable = false;
					FindAPebble.isRunning = false;
					resetRobot(); // new added
					System.out.println("routeSet Failed, try one more time");
					// no path sound

					// FindAPebble.roombacomm.control();
					// FindAPebble.roombacomm.playNote(50, 50); // C
					// FindAPebble.roombacomm.pause(200);
					// FindAPebble.roombacomm.playNote(32, 50); // G
					// FindAPebble.roombacomm.pause(200);

					// this will be modified later
					// FindAPebble.sndplayer.playCantfind();
					FindAPebble.sndplayer.playNopathto(finalTargetPebbleId);

				}
			}

		}

		else {
			System.out.println("The route is::");
			for(int i=0;i<backRouteArray.size();i++){
			System.out.println(backRouteArray.get(backRouteArray.size()-1-i).toString());
			}


			//snd
//			if (movingCount == 0) {
//				movingCount = 4;
//
//				FindAPebble.sndplayer.playMoving();
//			} else {
//				movingCount--;
//			}

			System.out.println("Current Target: " + currentTargetPebbleId);

			// new code for spot control
			if (isLost) {

				//snd
				isSearchingPlayed = false;

				// adjust here to set search number.
				if (searchCount >= 3) {
					isRouteSetAvailable = false;
					FindAPebble.isRunning = false;
					resetRobot();
					FindAPebble.roombacomm.control();
					FindAPebble.roombacomm.stop();
					FindAPebble.sndplayer.playLost();
					return;
				}

				if (searchTimerCount == 0) {
					searchTargetArray.clear();
					addSearchTarget(finalTargetPebbleId, searchTargetArray, backRouteArray);
				}

				System.out.println("searchTarget: " + searchTargetArray);
				for (int id : searchTargetArray) {
					if (channelInfoBuffer[id] > 0) {
						NodeRouting nr = new NodeRouting(id, finalTargetPebbleId,
								globalNodeList);
						if (nr.findPath()) {
							System.out
							.println("yes!!!!!! A new path is found!!! from "
									+ id + " to " + finalTargetPebbleId);
							System.out.println("get back from lost status......");
							System.out
							.println("Restart the route navigation......");

							ArrayList<BackupRouteNode> tmpArray = new ArrayList<BackupRouteNode>();
							for(BackupRouteNode node : backRouteArray){
								if(!node.visited)
									tmpArray.add(node);
							}
							backRouteArray.removeAll(tmpArray);

							nr.reslutDisplay();
							routeArray = nr.getResultList();
							System.err.println(routeArray);
							if (routeArray.size() > 0) {
								for (int m = 0; m < routeArray.size(); m++) {
									backRouteArray.add(0,new BackupRouteNode(
											routeArray.get(m), false));
								}
								System.out
								.println("BackupRouteArrayList initial info:");
								for (int m = 0; m < backRouteArray.size(); m++) {
									System.out.println(backRouteArray.get(m)
											.toString());
								}
							}

							FindAPebble.roombacomm.control();
							FindAPebble.roombacomm.stop();
							currentTargetPebbleId = id;
							resetLostInfo();
							isRouteSet = true;
							return;
						}
					}
				}

				/*
				System.out.println("Robot gets lost searching...");
				if (channelInfoBuffer[currentTargetPebbleId] > 0) {
					FindAPebble.roombacomm.control();
					FindAPebble.roombacomm.stop();
					resetLostInfo();
					// isLost = false;
					return;
				}

				// for (int i = 0; i < routeArray.size(); i++) {
				// int nextPebbleId = routeArray.get(i);
				// if (channelNumInSight(nextPebbleId,
				// channelInfoBuffer[nextPebbleId]) > 0) {
				// FindAPebble.roombacomm.control();
				// FindAPebble.roombacomm.stop();
				// // isLost = false;
				// //??? this should be i?
				// currentTargetPebbleId = i;
				// for (int j = i + 1; j < routeArray.size(); j++) {
				// routeArray.remove(j);
				// }
				// resetLostInfo();
				// return;
				// }
				// }
				// new code to support backtrack lost searching
				// if it gets lost, the robot will search back in the
				// preset path, use unvisited node as the restarting point.
				for (int i = 0; i < backRouteArray.size(); i++) {
					BackupRouteNode backupnode = backRouteArray.get(i);
					int nextPebbleId = backupnode.id;
					if (!backupnode.visited) {
						if (channelInfoBuffer[nextPebbleId]> 0) {
							FindAPebble.roombacomm.control();
							FindAPebble.roombacomm.stop();
							//
							currentTargetPebbleId = nextPebbleId;

							routeArray.clear();
							for (int j = 0; j < i; j++) {
								routeArray.add(backRouteArray.get(j).id);
							}
							resetLostInfo();
							return;
						}
					}
				}

				// recalculate route if fail to backtrack.
				// start from the node that is both in sight and not visited
				// in the precalculated route.
				ArrayList<Integer> restartList = new ArrayList<Integer>();
				for (int i = 0; i < localNodeList.size(); i++) {
					restartList.add(localNodeList.get(i).getId());
				}

				for (int i = 0; i < restartList.size(); i++) {
					int id = restartList.get(i);
					for (int j = 0; j < backRouteArray.size(); j++) {
						BackupRouteNode brn = backRouteArray.get(j);
						if (id == brn.id && brn.visited) {
							restartList.remove(i);
							break;
						}
					}
				}

				for (int i = 0; i < restartList.size(); i++) {
					int id = restartList.get(i);
					NodeRouting nr = new NodeRouting(id, finalTargetPebbleId,
							globalNodeList);
					if (nr.findPath()) {
						System.out
								.println("yes!!!!!! A new path is found!!! from "
										+ id + " to " + finalTargetPebbleId);
						System.out.println("get back from lost status......");
						System.out
								.println("Restart the route navigation......");
						//new code
						for(int j=0;j<backRouteArray.size();j++){
							if(!backRouteArray.get(j).visited){
								backRouteArray.remove(j);
							}
						}


						FindAPebble.roombacomm.control();

						FindAPebble.roombacomm.playNote(72, 10); // C
						FindAPebble.roombacomm.pause(200);
						FindAPebble.roombacomm.playNote(76, 10); // E
						FindAPebble.roombacomm.pause(200);
						FindAPebble.roombacomm.playNote(79, 10); // G
						FindAPebble.roombacomm.pause(200);

						FindAPebble.sndplayer.playGoal(finalTargetPebbleId);

						nr.reslutDisplay();
						routeArray = nr.getResultList();

						if (routeArray.size() > 0) {
							for (int m = 0; m < routeArray.size(); m++) {

								backRouteArray.add(0,new BackupRouteNode(
										routeArray.get(m), false));
							}
							System.out
									.println("BackupRouteArrayList initial info:");
							for (int m = 0; m < backRouteArray.size(); m++) {
								System.out.println(backRouteArray.get(m)
										.toString());
							}

						}

						// do not play route
						FindAPebble.sndplayer.playRoute(routeArray);

						currentTargetPebbleId = id;
						resetLostInfo();
						routeArray.remove(routeArray.size() - 1);
						isRouteSet = true;
						return;
					}
				}
				*/

				searchTimerCount++;
				// adjust here for the search time for each round.
				if (searchTimerCount >= 10) {
					searchCount++;
					searchTimerCount = 0;
					FindAPebble.roombacomm.control();
					FindAPebble.roombacomm.stop();
					FindAPebble.roombaSpot();
				}
				return;
			}



			// ////////////////////////////////////////////////////////////

			// added start

			if (isReached(channelInfoBuffer[currentTargetPebbleId])) {

				// reachCount++;

				// if(reachCount==2){
				// reachCount=0;
				// new code
				// set all the flag before current target in the route to be
				// visited.
				//uncomment this
				for (int m = 0; m <backRouteArray.size(); m++) {
					BackupRouteNode backNode = backRouteArray.get(m);
//					if (backNode.id != currentTargetPebbleId) {
////						backNode.visited = true;
//					} else {
//						backNode.visited = true;
//						break;
//					}
					if(backNode.id==currentTargetPebbleId){
						backNode.visited=true;
						break;
					}
				}

				if (currentTargetPebbleId == finalTargetPebbleId) {
					System.out.println("Robot arrived the final target!!!!!");

					// paly the sound to indicate that it arrives the final
					// target

					// this will be modified later
					FindAPebble.sndplayer.playArrived();

					FindAPebble.roombacomm.control();

					FindAPebble.roombacomm.playNote(79, 10); // G
					FindAPebble.roombacomm.pause(200);
					FindAPebble.roombacomm.playNote(76, 10); // E
					FindAPebble.roombacomm.pause(200);
					FindAPebble.roombacomm.playNote(72, 10); // C
					FindAPebble.roombacomm.pause(200);

//					FindAPebble.roombacomm.spin(360);
//					FindAPebble.roombacomm.stop();

					isRouteSetAvailable = false;
					FindAPebble.isRunning = false;
					resetRobot(); // new added

					//snd
					FindAPebble.sndplayer.play("newtask");
					isSpecifyPlayed = false;
					isProceedingPlayed = true;

				} else {
					System.out.println("Arrived at the Pebble:"
							+ currentTargetPebbleId);
					// notifyStopRobotEventListener(); // stop the robot to
					// avoid to hit the
					// pebble

					// play the sound to indicate it arrives the intermediate
					// target

					// FindAPebble.roombacomm.control();
					// FindAPebble.roombacomm.playNote(100, 10); // C
					// FindAPebble.roombacomm.pause(200);
					// FindAPebble.roombacomm.playNote(105, 10); // G
					// FindAPebble.roombacomm.pause(200);
					// FindAPebble.roombacomm.playNote(90, 10); // E
					// FindAPebble.roombacomm.pause(200);

					//

					if (!routeArray.isEmpty()) {
						int size = routeArray.size();

						int nextPebbleId = routeArray.get(size - 1);

						if (channelInfoBuffer[nextPebbleId] > 0) {

							// if(!checkPointPlayed)
							{
								FindAPebble.sndplayer.playCheckpoint();
							}

							currentTargetPebbleId = routeArray.get(size - 1);
							// isCurrentLostCount = 8;
							// isCurrentLostContinues = false;
							resetLostInfo();
							routeArray.remove(size - 1);
							targetPebbleDegreeArray.clear();
							System.out.println("next target is : "
									+ currentTargetPebbleId);
						} else {

							if (targetPebbleDegreeArray.size() == 3) {
								// notify drive robot
								if (!FindAPebble.busyAnglesEventListener) {
									notifyAnglesEventListeners(new AnglesEvent(
											targetPebbleDegreeArray
													.get(targetPebbleDegreeArray
															.size() - 1)));

									targetPebbleDegreeArray.clear();

									//snd
									moveCount++;
									FindAPebble.sndplayer.playMoving();
									isProceedingPlayed = false;

									if(moveCount%11==0){
										FindAPebble.sndplayer.play("slow");
									}
									else if(moveCount%21==0){
										FindAPebble.sndplayer.play("dontworry");
										moveCount = 0;
									}

								}
							} else {
								int localInfoSize1 = localNodeList.size();
								for (int i = 0; i < localInfoSize1; i++) {

									int localNodeId = localNodeList.get(i)
											.getId();
									if (currentTargetPebbleId == localNodeId) {
										targetPebbleDegreeArray
												.add(localNodeList.get(i)
														.getAngle());
									}
								}
							}
							// might have problems due to the next pebble is not
							// in
							// sight.
							// get closer to the pebble if next pebble is not in
							// sight.
						}
					}

				}

			}

			// not reached the current target.
			else {

				// reachCount=0;

				if (targetPebbleDegreeArray.size() == 3) {
					// notify drive robot
					if (!FindAPebble.busyAnglesEventListener) {
						FindAPebble.sndplayer.playMoving();
						isProceedingPlayed = false;

						notifyAnglesEventListeners(new AnglesEvent(
								targetPebbleDegreeArray
										.get(targetPebbleDegreeArray.size() - 1)));

						targetPebbleDegreeArray.clear();

						//snd
						moveCount++;
						if(moveCount%7==0){
							FindAPebble.sndplayer.play("slow");
						}
						else if(moveCount%13==0){
							FindAPebble.sndplayer.play("dontworry");
							moveCount = 0;
						}

					}
				} else {

					int localInfoSize1 = localNodeList.size();

					if (!isLostContinues) {
						if (localInfoSize1 == 0) {
							isLostCount = 7;
							isLostContinues = true;
							isLostCount--;
							return;
						}
					} else {
						if (localInfoSize1 == 0) {
							isLostCount--;
							if (isLostCount == 0) {
								isLostCount = 7;
								// FindAPebble.sndplayer.playLost();
								// FindAPebble.roombaSpot(30);
								for (int m = 0; m < 5; m++) {
									System.out
											.println("---------robot cannot see anything and start searching."
													+ "---------------------");
								}
								// isLost = true;
								// ////////////

								// un-comment this
								FindAPebble.roombaSpot();
								isLost = true;

								// isRouteSetAvailable = false;
								// FindAPebble.isRunning = false;
								// resetRobot(); // new added
								// isLostContinues = false;

								return;
							}
						} else {
							isLostContinues = false;
						}

					}

					for (int i = 0; i < localInfoSize1; i++) {

						int localNodeId = localNodeList.get(i).getId();

						if (!isCurrentLostContinues) {
							if (channelInfoBuffer[currentTargetPebbleId] == 0) {
								isCurrentLostContinues = true;
								isCurrentLostCount = 7;
								isCurrentLostCount--;
								return;
							}
						} else {
							if (channelInfoBuffer[currentTargetPebbleId] == 0) {
								isCurrentLostCount--;
								if (isCurrentLostCount == 0) {
									// FindAPebble.sndplayer.playLost();
									// System.out.println("");

									// FindAPebble.roombaSpot(30);
									// System.out.println("robot is lost and start searching.");
									// isLost = true;
									// ////////////

									isCurrentLostCount = 7;
									// FindAPebble.sndplayer.playLost();
									for (int m = 0; m < 5; m++) {
										System.out
												.println("---------robot cannot see current pebble and start searching."
														+ "---------------------");
									}

									FindAPebble.roombaSpot();
									isLost = true;

									// uncomment this to undo
									// isRouteSetAvailable = false;
									// FindAPebble.isRunning = false;
									// resetRobot(); // new added
									// isCurrentLostContinues = false;

									return;
								}

							} else {
								isCurrentLostContinues = false;
							}

						}

						if (currentTargetPebbleId == localNodeId) {
							targetPebbleDegreeArray.add(localNodeList.get(i)
									.getAngle());
						}

						if (!routeArray.isEmpty()) {
							int routeArraySize = routeArray.size();
							int nextPebbleId = routeArray
									.get(routeArraySize - 1);

							if (localNodeId == nextPebbleId
									&& (channelInfoBuffer[localNodeId] >= 3)) {
								System.out
										.println("Find the next pebble in the route, go to next pebble:"
												+ nextPebbleId);

								// next target speak is not played
								// FindAPebble.sndplayer.playGoal(nextPebbleId);

								FindAPebble.sndplayer.playFoundnext();

								// notifyStopRobotEventListener(); // stop the
								// robot to
								// avoid to hit
								// the pebble
								currentTargetPebbleId = nextPebbleId;
								routeArray.remove(routeArraySize - 1);
								targetPebbleDegreeArray.clear();

							}

						}

					}

				}

			}

		}

	}

	public boolean isReached(int numberOfChannels) {
		int reachChannelNum = 6; // how many channel to decide a robot reached a
									// pebble

		if (numberOfChannels >= reachChannelNum)
			return true;

		return false;
	}

	public int channelNumInSight(int id, int[] array) {
		int count = 0;
		for (int i = 0; i < 8; i++) {
			if (array[i] == id)
				count++;
		}

		return count;
	}

	private void addSearchTarget(int targetID,
			ArrayList<Integer> searchTargetArray,
			ArrayList<BackupRouteNode> backRouteArray) {
		if (searchTargetArray.contains(targetID))
			return;
		if (isVisited(targetID, backRouteArray))
			return;

		searchTargetArray.add(targetID);

		Node targetNode = null;
		for (Node node : globalNodeList.nodeList) {
			if (node.getId() == targetID) {
				targetNode = node;
				break;
			}
		}
		if (targetNode == null)
			return;

		for (Neibour neibour : targetNode.getNeibours())
			addSearchTarget(neibour.getId(), searchTargetArray, backRouteArray);
	}

	private boolean isVisited(int targetID,
			ArrayList<BackupRouteNode> backRouteArray) {
		for (BackupRouteNode node : backRouteArray) {
			if (node.id == targetID && node.visited)
				return true;
		}

		return false;
	}

	public static void main(String args[]) {
		dataDrivenController blockData = new dataDrivenController();
		blockData.initDecoder();
	}

}
